#include <iostream>
#include <cmath>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <assert.h>

#include "position_solutions.h"

#define NDEBUG
using namespace boost::numeric;

using std::sin;
using std::cos;
using std::atan;
using std::atan2;


typedef ublas::vector<double> uvector;





double g2r(double val)
{
  return val/180.0*PI;
}
double r2g(double val)
{
  return val*180.0/PI;
}



std::vector<uvector >
quantise_line(uvector a,
	      uvector b,
	      double delta)
{
  std::vector<uvector > quants;
  
  assert(delta>0);
  
  uvector line=b-a;
  uvector line_id=line/norm_2(line);
  long count=(long)(norm_2(line)/delta);
  for(long i=0; i<count; i++){
    a+=line_id*delta;
    quants.push_back(a);
  }
  quants.push_back(b);
  return quants;
}

matrix_vector
quantise_translate(matr A, matr B, double delta)
{
  matrix_vector quants;
  uvector a(3);
  // a = subrange (A, 0, 3, 3, 4);
  a(0)=A(0,3);
  a(1)=A(1,3);
  a(2)=A(2,3);
  uvector b(3);
  // b = subrange (B, 0, 3, 3, 4);
  b(0)=B(0,3);
  b(1)=B(1,3);
  b(2)=B(2,3);
  std::vector<uvector >
    line_quants=quantise_line(a,b,delta);

  //linear interpolation params for rotattion
  uvector begin_angles=R2oat(A);
  uvector end_angles=R2oat(B);
  uvector angles_delta=
    (end_angles-begin_angles)/(double)line_quants.size();
    
  for(long i=0; i<line_quants.size(); i++){
    //rotation interpolation
    
    begin_angles+=angles_delta;
    matr T=oat2R(begin_angles);

    //translation interpolation
    T(0,3)=line_quants[i](0);
    T(1,3)=line_quants[i](1); 
    T(2,3)=line_quants[i](2);
    quants.push_back(T);
  }
  
  return quants;
}

matrix_vector
quantise_translate_only(matr A, matr B, double delta)
{
  matrix_vector quants;
  uvector a(3);
  // a = subrange (A, 0, 3, 3, 4);
  a(0)=A(0,3);
  a(1)=A(1,3);
  a(2)=A(2,3);
  uvector b(3);
  // b = subrange (B, 0, 3, 3, 4);
  b(0)=B(0,3);
  b(1)=B(1,3);
  b(2)=B(2,3);
  std::vector<uvector >
    line_quants=quantise_line(a,b,delta);
  for(long i=0; i<line_quants.size(); i++){
    //subrange (A, 0, 3, 3, 4)=line_quants[i];
    A(0,3)=line_quants[i](0);
    A(1,3)=line_quants[i](1);
    A(2,3)=line_quants[i](2);
    quants.push_back(A);
  }
  
  return quants;
}
    
    

matrix_vector
forward_solution_A( double q1,
		    double q2,
		    double q3,
		    double q4,
		    double q5,
		    double q6)
{
  matrix_vector v;
 
  const double s1=sin(q1),
    s2=sin(q2),
    s3=sin(q3),
    s4=sin(q4),
    s5=sin(q5),
    s6=sin(q6),

    c1=cos(q1),
    c2=cos(q2),
    c3=cos(q3),
    c4=cos(q4),
    c5=cos(q5),
    c6=cos(q6);

  /* A1=
     c1 0 -s1 0
     s1 0  c1 0
     0 -1  0  0
     0  0  0  1;
  */
  ublas::matrix<double> A1=ublas::identity_matrix<double>(4);
  A1(0,0)=c1; A1(0,2)=-s1;
  A1(1,0)=s1; A1(1,1)=0; A1(1,2)=c1;
  A1(2,1)=-1; A1(2,2)=0;
 
  /* A2=
     c2 -s2 0 a2*c2
     s2  c2 0 a2*s2
     0   0  1 d2
     0   0  0 1];
  */  
  ublas::matrix<double> A2=ublas::identity_matrix<double>(4);
  A2(0,0)=c2; A2(0,1)=-s2; A2(0,3)=a2*c2;
  A2(1,0)=s2; A2(1,1)=c2;  A2(1,3)=a2*s2;
  A2(2,3)=d2;

  /* A3=
     c3 0 s3  a3*c3
     s3 0 -c3 a3*s3
     0  1 0   0
     0  0 0   1]; 
  */
  ublas::matrix<double> A3=ublas::identity_matrix<double>(4);
  A3(0,0)=c3; A3(0,2)=s3; A3(0,3)=a3*c3;
  A3(1,0)=s3; A3(1,1)=0; A3(1,2)=-c3; A3(1,3)=a3*s3;
  A3(2,1)=1; A3(2,2)=0;

  /* A4=[c4 0 -s4 0  
     s4 0  c4 0  
     0 -1  0 d4 
     0  0  0  1];
  */
  ublas::matrix<double> A4=ublas::identity_matrix<double>(4);
  A4(0,0)=c4; A4(0,2)=-s4; 
  A4(1,0)=s4; A4(1,1)=0; A4(1,2)=c4; 
  A4(2,1)=-1; A4(2,2)=0; A4(2,3)=d4;

  /*
    A5=
    c5 0 s5 0
    s5 0 -c5 0
    0  1  0 0
    0  0  0 1];
  */
  ublas::matrix<double> A5=ublas::identity_matrix<double>(4);
  A5(0,0)=c5; A5(0,2)=s5;
  A5(1,0)=s5; A5(1,1)=0; A5(1,2)=-c5;
  A5(2,1)=1; A5(2,2)=0;

  /*
    A6=c6 -s6 0 0
    s6 c6 0 0
    0 0 1 d6
    0 0 0 1];
  */
  ublas::matrix<double> A6=ublas::identity_matrix<double>(4);
  A6(0,0)=c6; A6(0,1)=-s6; 
  A6(1,0)=s6; A6(1,1)=c6; 
  A6(2,3)=d6;

  v.push_back(A1);
  v.push_back(A2);
  v.push_back(A3);
  v.push_back(A4);
  v.push_back(A5);
  v.push_back(A6);

  return v;
}

inline matrix_vector
forward_solution_A( std::vector<double> Q)
{
  return forward_solution_A(Q[0],Q[1],Q[2],
			    Q[3],Q[4],Q[5]);
}


matrix_vector
forward_solution_T( double q1,
		    double q2,
		    double q3,
		    double q4,
		    double q5,
		    double q6,
		    matr T0)
{
 matrix_vector a=
   forward_solution_A(q1,q2,q3,q4,q5,q6);

 matrix_vector t;  

 ublas::matrix<double> T=T0;
 
 
 for(int i=0; i<a.size(); i++){
   T=prod(T,a[i]);
   t.push_back(T);
 }
 
 return t;
}

matrix_vector
forward_solution_T( std::vector<double> Q,matr T0)
{
  return forward_solution_T(Q[0],Q[1],Q[2],
			    Q[3],Q[4],Q[5],T0);
}

/*double atan2(double a, double b)
{
  //from Yuschenko...

  //brute force
  //if(a==0 && b==0) atan2
  //  return 0;
  
  
  if(b>=0)
    return  (b!=0)?atan(a/b):PI/2;
  else{
    if(a>0)
      return atan(a/b)+PI;
    else
      return atan(a/b)-PI;
  }
  
} */

/**
   inversion of translation-rotation matrix
   refer [Yuschenko],56
*/
matr special_inverse(matr T)
{
 ublas::matrix_range<matr> R= subrange (T, 0, 3, 0, 3);
 R=trans(R);
 ublas::matrix_range<matr> P= subrange (T, 0, 3, 3, 4);
 P=prod(-R,P);
 return T;
}

point to_angles(const point &O, const point &D, double t)
{
  point p=O;
  
  point d=D.norm();

  p.o=acos(d.x);
  if(d.y<0)
    p.o=2*PI-p.o;

  d=point(0,sqrt(d.x*d.x+d.y*d.y),d.z).norm();

  p.a=acos(d.z);
  if(d.y<0)
    p.a=2*PI-p.a;

  p.t=t;

  return p;
}


matr fit_orient(const point &O, const point &D, double t)
{
  point d=D.norm();

  double o=acos(d.x);
  if(d.y<0)
    o=2*PI-o;

  d=point(0,sqrt(d.x*d.x+d.y*d.y),d.z).norm();

  double a=acos(d.z);
  if(d.y<0)
    a=2*PI-a;

  return build_trans(O.x,O.y,O.z,o,a,t);
}

  

double work_sphere_r()
{
  const double L=sqrt(d4*d4+a3*a3);
  return sqrt(d2*d2+pow(d6+a2+L,2));
}

double dead_cylinder_r()
{
  return d2-d6;
}


std::vector<double>
reverse_solution(matr T,
		 modifier k1,
		 modifier k2,
		 modifier W,
		 matr T0)
{

  //translating to robot own CS
  T=prod(special_inverse(T0),T);

  // checking workspace boundaries
 ublas::matrix_column<matr> P(T,3);
  if(norm_2(P)>=work_sphere_r()){
   std::ostringstream ost;
    ost<<"Requested point is out of working sphere!"<<std::endl
       <<"x="<<P(0)<<" y="<<P(1)<<" z="<<P(2)<<" maxR="<<(int)work_sphere_r();
    throw ost.str();
  }
  if(sqrt(P(0)*P(0)+P(1)*P(1))<=dead_cylinder_r()){
    std::ostringstream ost;
    ost<<"Requested point is in the dead zone"<<std::endl
       <<"x="<<P(0)<<" y="<<P(1)<<" minR="<<(int)dead_cylinder_r()+1;
    throw ost.str(); 
  }

  // check done
  // now let's brutally calculate q1-q6
  
  const double
    xx=T(0,0), yx=T(0,1), zx=T(0,2), px=T(0,3)-d6*T(0,2),
    xy=T(1,0), yy=T(1,1), zy=T(1,2), py=T(1,3)-d6*T(1,2),
    xz=T(2,0), yz=T(2,1), zz=T(2,2), pz=T(2,3)-d6*T(2,2);


  
  std::vector<double> q(6,0);

  assert((px*px+py*py-d2*d2)>=0);
  q[0]=2*atan((-px+k1*sqrt(px*px+py*py-d2*d2))/(py+d2));

  double s1=sin(q[0]), c1=cos(q[0]);
  double Q=px*c1+py*s1;
  double A=(Q*Q+pz*pz+a2*a2-a3*a3-d4*d4)/2/a2;

  assert((pz*pz+Q*Q-A*A)>=0);
  q[1]=2*atan((-pz+k2*sqrt(pz*pz+Q*Q-A*A))/(Q+A));

  double s2=sin(q[1]), c2=cos(q[1]);
  double B=px*c1+py*s1-a2*c2;
  double C=pz+a2*s2;
  double s23=(d4*B-a3*C)/(d4*d4+a3*a3);
  double c23=(d4*C+a3*B)/(d4*d4+a3*a3);

  q[2]=atan2(s23,c23)-q[1];

  q[3]=atan2(W*(-s1*zx+c1*zy),W*(c1*c23*zx+s1*c23*zy-s23*zz));

  double s4=sin(q[3]), c4=cos(q[3]);
  double s5=(c1*c23*c4-s1*s4)*zx+(s1*c23*c4+c1*s4)*zy-c4*s23*zz;
  double c5=c1*s23*zx+s1*s23*zy+c23*zz;

  q[4]=atan2(s5,c5);

  double s6=(-s1*c4-c1*c23*s4)*xx+(c1*c4-s1*c23*s4)*xy+s4*s23*xz;
  double c6=(-s1*c4-c1*c23*s4)*yx+(c1*c4-s1*c23*s4)*yy+s4*s23*yz;

  q[5]=atan2(s6,c6);

  return q;
}


matr Rotx(double phi)
{
 double c=cos(phi), s=sin(phi);

  matr A=ublas::identity_matrix<double>(4);

  A(1,1)=c; A(1,2)=-s;
  A(2,1)=s; A(2,2)=c;

  return A;
}


matr Roty(double phi)
{
  double c=cos(phi), s=sin(phi);

  matr A=ublas::identity_matrix<double>(4);

  A(0,0)=c; A(0,2)=s;
  A(2,0)=-s; A(2,2)=c;

  return A;
}

matr Rotz(double phi)
{
  double c=cos(phi), s=sin(phi);

  matr A=ublas::identity_matrix<double>(4);

  A(0,0)=c; A(0,1)=-s;
  A(1,0)=s; A(1,1)=c;

  return A;
}



matr oat2R(double o, double a, double t){
  matr P=prod(Rotz(o),Roty(a));
  return prod(P,Rotz(t));
 
}



matr oat2R(uvector a){
  return oat2R(a(0),a(1),a(2));
}

uvector R2oat(matr T)
{
  //from Yuschenko, 95
  const double
    xx=T(0,0), yx=T(0,1), zx=T(0,2),
    xy=T(1,0), yy=T(1,1), zy=T(1,2),
    xz=T(2,0), yz=T(2,1), zz=T(2,2);
  
  uvector ang(3);

  // 'o' angle
  ang(0)=atan2(zy,zx);

  double so=sin(ang(0)), co=cos(ang(0));

  // 'a' angle
  ang(1)=atan2(zx*co+zy*so,zz);

  // 't' angle
  ang(2)=atan2(-xx*so+xy*co, -yx*so+yy*co);


  return ang;
}

matr build_trans(double x, double y, double z,
		 double o, double a, double t)
{
  matr T=oat2R(o,a,t);
  T(0,3)=x;
  T(1,3)=y;
  T(2,3)=z;

  return T;
}

matr build_trans(point p)
{
  return build_trans(p.x,p.y,p.z,
		     p.o,p.a,p.t);
}


point point_dir(const point &p)
{
  matr T=oat2R(p.o,p.a,p.t);
  return point(T(0,2),T(1,2),T(2,2));
}


std::vector<double> T2xyz(matr T)
{
  std::vector<double> P;
  for(int i=0; i<3; i++)
    P.push_back(T(i,3));
  uvector oat= R2oat(T);
  for(int i=0; i<3; i++)
    P.push_back(oat(i));
  return P;
  
}

point T2pnt(matr T)
{
  std::vector<double> P=T2xyz(T);
  return point(P[0],P[1],P[2],P[3],P[4],P[5]);
}

  

matr build_trans(std::vector<double> P)
{
 
  return build_trans(P[0],P[1],P[2],P[3],P[4],P[5]);
}

    
   
  




  

